import math
import numpy as np
from scipy.spatial.transform import Rotation as R
from pyquaternion import Quaternion
 
deg2rad = 3.1415926 / 180.0

def rpy2RotCnb(roll, pitch, yaw):
    roll = roll * deg2rad
    pitch = pitch * deg2rad
    yaw = yaw * deg2rad
    # roll : y, pitch : x, yaw : z
    Croll = np.array([[math.cos(roll),                0,      - math.sin(roll)],
                      [             0,              1.0,                     0],
                      [math.sin(roll),                0,        math.cos(roll)]])
    Cpitch = np.array([[          1.0,                0,                      0],
                       [            0,   math.cos(pitch),       math.sin(pitch)],
                       [            0, - math.sin(pitch),      math.cos(pitch)]])
    Cyaw = np.array([[   math.cos(yaw),    math.sin(yaw),       0],
                     [ - math.sin(yaw),    math.cos(yaw),       0],
                     [               0,                0,     1.0]])
    Cbn = np.matmul(Croll, np.matmul(Cpitch, Cyaw))
    Cnb = Cbn.transpose()

    return Cnb

def blh2RotCen(B,L):
    B = B * deg2rad
    L = L * deg2rad 

    Cen = np.zeros((3,3),dtype='float')
    Cen[0,0] = - math.sin(B) * math.cos(L)
    Cen[0,1] = - math.sin(L)
    Cen[0,2] = - math.cos(B) * math.cos(L)
    Cen[1,0] = - math.sin(B) * math.sin(L)
    Cen[1,1] = math.cos(L)
    Cen[1,2] = - math.cos(B) * math.sin(L)
    Cen[2,0] = math.cos(B)
    Cen[2,1] = 0
    Cen[2,2] = math.sin(B)

    return Cen

def rot2quat(rot):
    # quat = [x, y, z, w]
    # quat_ = list(Quaternion(matrix=rot))
    # quat = [quat_[1], quat_[2], quat_[3], quat_[0]]
    # # print(quat)

    r = R.from_dcm(rot)
    qvec_nvm = r.as_quat()
    quat_ = np.array(qvec_nvm)
    quat = [quat_[0], quat_[1], quat_[2], quat_[3]]
    # print(quat)

    return quat

def quat2euler(quat):
    r = R.from_quat(quat)
    euler__ = r.as_euler('xyz', degrees=True)
    euler_ = np.array(euler__)
    euler = [euler_[0], euler_[1], euler_[2]]
    return euler

def euler2rot(euler):
    r = R.from_euler('xyz', euler, degrees=True)
    rot_ = r.as_dcm()
    rot = np.array(rot_)
    return rot

def rot2euler(rot):
    return quat2euler(rot2quat(rot))